/*
 *  Mfuns.c
 *  SSE
 *
 *  Created by Matt Roman on 10/12/07.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "Mfuns.h"

// calculates the unit vector from point abc1 to point abc2 and puts the new vector into pointer *vect
// returns the distance from point abc1 to point abc2
float PntstoVector(Point3f *a, Point3f *b, Vector3f *vect)
{
	double unit;
	
	vect->a = b->x - a->x;
	vect->b = b->y - a->y;
	//vect->c = b->z - a->z;
	vect->c = 0;
	unit = sqrt((vect->a*vect->a) + (vect->b*vect->b) + (vect->c*vect->c));
	
	vect->a /= unit;
	vect->b /= unit;
	vect->c /= unit;
	
	return unit;
}

double AngleBetweenUnitVector(Vector3f *V1, Vector3f *V2)
{
	double dot;
	Vector3f V3;
	
	dot = (V1->a * V2->a) + (V1->b * V2->b) + (V1->c * V2->c);
	if(dot > 1.0) return 0.0;
	else if(dot < -1.0) return PI;
	
	V3.a = V1->b*V2->c - V1->c*V2->b;
	V3.b = V1->c*V2->a - V1->a*V2->c;
	V3.c = V1->a*V2->b - V1->b*V2->a;
	
	if(V3.c >= 0) return acos(dot);
	else return -acos(dot);
}

void PntstoVectorPtr(GLfloat *p1, GLfloat *p2, Vector3f *vect)
{
	vect->a = *p2 - *p1;
	vect->b = *(p2+1) - *(p1+1);
	vect->c = *(p2+2) - *(p1+2);
}

// calculates the normalized cross product of v1 to v2 and places it in v3
float NormalizeCross(Vector3f *v1, Vector3f *v2, Vector3f *v3)
{
	float r;
	// a x b = ((a2b3 - a3b2),(a3b1 - a1b3),(a1b2 - a2b1))
	
	v3->a = v1->b*v2->c - v1->c*v2->b;
	v3->b = v1->c*v2->a - v1->a*v2->c;
	v3->c = v1->a*v2->b - v1->b*v2->a;
	
	r = sqrt((v3->a*v3->a) + (v3->b*v3->b) + (v3->c*v3->c));
	
	v3->a /= r;
	v3->b /= r;
	v3->c /= r;
	
	//if(v3->c >= 0) return 1;
	//else return -1;
	return v3->c;
}

// returns the distance between two points in 3 space, p and q
double magnitude3d(Point3f *p,Point3f *q)
{
	return sqrt((p->x - q->x)*(p->x - q->x) + (p->y - q->y)*(p->y - q->y) + (p->z - q->z)*(p->z - q->z));
}

double magnitude3v(Vector3f *v)
{
	return sqrt(v->a*v->a + v->b*v->b + v->c*v->c);
}

int DistancePointLine(Point3f *point, Point3f *start, Point3f *end, double *distance)
{
	float lineMag;
	float U;
	Point3f Intersect;
	
	lineMag = magnitude3d(end,start);
	
	U = (((point->x - start->x)*(end->x - start->x)) + ((point->y - start->y)*(end->y - start->y))+
		 ((point->z - start->z)*(end->z - start->z)))/(lineMag*lineMag);
	
	Intersect.x = start->x + U*(end->x - start->x);
	Intersect.y = start->y + U*(end->y - start->y);
	Intersect.z = start->z + U*(end->z - start->z);
	
	*distance = magnitude3d(point,&Intersect);
	if(U < 0) return -1;
	else if(U > 1) return -1; // point does not fall within line segment
	
	return 1;
}

///////////////////////////////////////////////////////////////////////////////
// Calculates if two lines intersect eachother and returns the point at which they do
int SegmentIntersect(Point3f *p1, Point3f *p2, Point3f *p3, Point3f *p4, Point3f *intersect)
{
	double z1,z2;
	double m1,m2,b1,b2;
	int s1,s2;
	
	/////////////////////////////////////////////
	// Quick bounding box rejection check
	if(!(MAXIMUM(p1->x, p2->x) > MINIMUM(p3->x, p4->x) && 
		 MAXIMUM(p3->x, p4->x) > MINIMUM(p1->x, p2->x) &&
		 MAXIMUM(p1->y, p2->y) > MINIMUM(p3->y, p4->y) &&
		 MAXIMUM(p3->y, p4->y) > MINIMUM(p1->y, p2->y)))
	{
		intersect->x = p2->x;
		intersect->y = p2->y;
		intersect->z = p2->z;
		return 0;
	}
	
	/////////////////////////////////////////////
	// if any of the segment end points are the same return 0
	// don't want to deal with vertices that the robot can already reach
	//if((p3 == p1) || (p3 == p2) || (p4 == p1) || (p4 == p2)) return 0;
	/////////////////////////////////////////////
	// do they straddle each other?
	if((z1 = ((p3->x - p1->x)*(p2->y - p1->y)) - ((p3->y - p1->y)*(p2->x - p1->x))) < 0)
		s1 = -1;
	else if(z1 > 0)
		s1 = 1;
	else
		s1 = 0;
	
	if((z2 = ((p4->x - p1->x)*(p2->y - p1->y)) - ((p4->y - p1->y)*(p2->x - p1->x))) < 0)
		s2 = -1;
	else if(z2 > 0)
		s2 = 1;
	else
		s2 = 0;
	
	if((s1 != s2) && s1 != 0 && s2 != 0) // if side 1 and side 2 are different the lines straddle
	{										// if side 1 or side 2 are zero then they are assumed not to intersect
		m1 = (p2->y - p1->y)/(p2->x - p1->x);
		b1 = p1->y - m1*p1->x;
		
		m2 = (p4->y - p3->y)/(p4->x - p3->x);
		b2 = p3->y - m2*p3->x;
		
		intersect->x = (b2-b1)/(m1-m2);
		intersect->y = m1*intersect->x + b1;
		intersect->z = 0;
		
		if(intersect->x >= MINIMUM(p1->x, p2->x) && intersect->x <= MAXIMUM(p1->x, p2->x)) return 1;
	}
	intersect->x = p2->x;
	intersect->y = p2->y;
	intersect->z = p2->z;
	
	return 0;
}

// returns the type of intersection made by the segment p1-p2 about a circle with center at cc and a radius of r
// returns 0 if no intersection
// returns 1 if the segment is tangent or only intersects once at point Isect1
// returns 2 if the segment intersects at point Isect2
// returns 3 if the segment intersects twice at point Isect1 and Isect2
int SegmentIntersectArc(Point3f *p1, Point3f *p2, Point3f *cc, float r, Point3f *Isect1, Point3f *Isect2)
{
	int sng;
	int type;
	float discrim;
	float dr,dx,dy,D;
	float p1x = (p1->x - cc->x);	// translate everything to (0,0) 
	float p1y = (p1->y - cc->y);
	float p2x = (p2->x - cc->x);
	float p2y = (p2->y - cc->y);
	
	Isect1->handy = 0;
	Isect2->handy = 0;
	
	dx = p2x - p1x;
	dy = p2y - p1y;
	dr = sqrt(SQ(dx) + SQ(dy));
	D = p1x*p2y - p2x*p1y;
	
	if(dy < 0) sng = -1;
	else sng = 1;
	
	discrim = SQ(r)*(SQ(dx) + SQ(dy)) - SQ(D);
	
	if(discrim < 0) type = 0;	// avoid imaginary numbers from lines that don't intersect the arc
	else
	{
		type = 0;
		
		Isect1->x = cc->x + ((D*dy+sng*dx*sqrt(discrim))/SQ(dr));
		Isect1->y = cc->y + ((-D*dx+fabs(dy)*sqrt(discrim))/SQ(dr));
		Isect1->z = 0;
		
		Isect2->x = cc->x + ((D*dy-sng*dx*sqrt(discrim))/SQ(dr));
		Isect2->y = cc->y + ((-D*dx-fabs(dy)*sqrt(discrim))/SQ(dr));
		Isect2->z = 0;
		
		if(Isect1->x >= MINIMUM(p1->x,p2->x) && 
		   Isect1->x <= MAXIMUM(p1->x,p2->x) &&
		   Isect1->y >= MINIMUM(p1->y,p2->y) &&
		   Isect1->y <= MAXIMUM(p1->y,p2->y)){
			Isect1->handy = 1;
			type = 1;						// if the first point is inbetween the line segment ends
			if(discrim == 0) return 1;		// if the line is tangent
		}
		
		if(Isect2->x >= MINIMUM(p1->x,p2->x) &&
		   Isect2->x <= MAXIMUM(p1->x,p2->x) &&
		   Isect2->y >= MINIMUM(p1->y,p2->y) &&
		   Isect2->y <= MAXIMUM(p1->y,p2->y)) {
			Isect2->handy = 1;
			if(type) type = 3;				// if the line segment crosses at two points between ends
			else type = 2;					// if the second point is between the line segment ends
		}
	}
	return type;
}